#include <rclcpp/rclcpp.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <geometry_msgs/msg/point.hpp>
#include <nav_msgs/msg/path.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2/utils.h>
#include <queue>
#include <cmath>
#include <unordered_set>
#include <mutex>
#include <memory>
#include <vector>

// 网格坐标结构体
struct GridCoord
{
     int x, y;

     GridCoord(int x = 0, int y = 0) : x(x), y(y) {}

     bool operator==(const GridCoord &other) const
     {
          return x == other.x && y == other.y;
     }

     bool operator<(const GridCoord &other) const
     {
          return (x < other.x) || (x == other.x && y < other.y);
     }
};

// 哈希函数特化
namespace std
{
     template <>
     struct hash<GridCoord>
     {
          size_t operator()(const GridCoord &coord) const
          {
               return static_cast<size_t>(coord.x) * 131071 + static_cast<size_t>(coord.y);
          }
     };
}

// A*节点
struct AStarNode
{
     GridCoord coord;
     double g_cost; // 从起点到当前节点的代价
     double h_cost; // 启发式代价（到目标的估计）

     double f_cost() const { return g_cost + h_cost; }

     // 父节点指针
     std::shared_ptr<AStarNode> parent;

     AStarNode(GridCoord coord, double g, double h,
               std::shared_ptr<AStarNode> parent = nullptr)
         : coord(coord), g_cost(g), h_cost(h), parent(parent) {}
};

// 用于优先队列的比较函数
struct CompareAStarNode
{
     bool operator()(const std::shared_ptr<AStarNode> &a,
                     const std::shared_ptr<AStarNode> &b)
     {
          return a->f_cost() > b->f_cost();
     }
};

class PathPlanner : public rclcpp::Node
{
public:
     PathPlanner() : Node("path_planner"),
                     grid_size_(1.0),
                     goal_tolerance_(0.3)
     {

          // 初始化目标点 (5, 5)
          goal_position_.x = 3.0;
          goal_position_.y = 5.0;

          // 订阅传感器数据
          odom_sub_ = create_subscription<nav_msgs::msg::Odometry>(
              "/odom", 10,
              std::bind(&PathPlanner::odom_callback, this, std::placeholders::_1));

          lidar_sub_ = create_subscription<sensor_msgs::msg::LaserScan>(
              "/lidar/scan", 10,
              std::bind(&PathPlanner::lidar_callback, this, std::placeholders::_1));

          radar_sub_ = create_subscription<sensor_msgs::msg::LaserScan>(
              "/radar/scan", 10,
              std::bind(&PathPlanner::radar_callback, this, std::placeholders::_1));

          // 控制发布器
          cmd_vel_pub_ = create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);

          // 路径发布器
          path_pub_ = create_publisher<nav_msgs::msg::Path>("/astar_planning_path", 10);

          // 路径规划定时器
          planning_timer_ = create_wall_timer(
              std::chrono::milliseconds(500),
              std::bind(&PathPlanner::planning_callback, this));

          RCLCPP_INFO(get_logger(), "Path planner initialized. Goal: (%.1f, %.1f)",
                      goal_position_.x, goal_position_.y);
     }

private:
     // 传感器回调函数
     void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg)
     {
          std::lock_guard<std::mutex> lock(data_mutex_);
          current_position_ = msg->pose.pose.position;
          current_yaw_ = tf2::getYaw(msg->pose.pose.orientation);
     }

     void lidar_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg)
     {
          std::lock_guard<std::mutex> lock(data_mutex_);
          update_obstacles_from_scan(msg, true);
     }

     void radar_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg)
     {
          std::lock_guard<std::mutex> lock(data_mutex_);
          update_obstacles_from_scan(msg, false);
     }

     // 从扫描数据更新障碍物
     void update_obstacles_from_scan(const sensor_msgs::msg::LaserScan::SharedPtr scan, bool is_lidar)
     {

          for (size_t i = 0; i < scan->ranges.size(); ++i)
          {
               if (std::isinf(scan->ranges[i]) || std::isnan(scan->ranges[i]))
                    continue;

               double angle = scan->angle_min + i * scan->angle_increment;
               double global_angle = angle + current_yaw_;

               // 转换到全局坐标系
               double obs_x = current_position_.x + scan->ranges[i] * cos(global_angle);
               double obs_y = current_position_.y + scan->ranges[i] * sin(global_angle);

               // 转换为网格坐标
               GridCoord obs_cell = world_to_grid(obs_x, obs_y);

               // 标记为障碍物
               obstacles_.insert(obs_cell);
          }
     }

     // 路径规划回调
     void planning_callback()
     {
          std::lock_guard<std::mutex> lock(data_mutex_);

          // 获取当前位置的网格坐标
          GridCoord start = world_to_grid(current_position_.x, current_position_.y);
          GridCoord goal = world_to_grid(goal_position_.x, goal_position_.y);

          // 检查是否到达目标
          if (distance_between_points(current_position_, goal_position_) < goal_tolerance_)
          {
               stop_robot();
               RCLCPP_INFO(get_logger(), "Goal reached!");
               planning_timer_->cancel();
               return;
          }

          // 执行A*路径规划
          std::vector<GridCoord> grid_path = a_star_planning(start, goal);

          if (!grid_path.empty())
          {
               // 发布完整路径
               publish_path(grid_path);

               // 选择路径上的下一个点
               if (grid_path.size() > 1)
               {
                    GridCoord next_point = grid_path[1]; // 跳过起点
                    auto [world_x, world_y] = grid_to_world(next_point.x, next_point.y);

                    // 计算控制指令
                    geometry_msgs::msg::Twist cmd_vel = calculate_control(world_x, world_y);
                    cmd_vel_pub_->publish(cmd_vel);
               }
          }
          else
          {
               stop_robot();
               RCLCPP_ERROR(get_logger(), "No valid path found!");
          }
     }

     // A*路径规划算法
     std::vector<GridCoord> a_star_planning(const GridCoord &start, const GridCoord &goal)
     {
          // 优先队列（最小堆）
          using NodePtr = std::shared_ptr<AStarNode>;
          std::priority_queue<NodePtr, std::vector<NodePtr>, CompareAStarNode> open_set;

          // 节点存储（避免重复创建）
          std::unordered_map<GridCoord, NodePtr> node_map;

          // 起点初始化
          auto start_node = std::make_shared<AStarNode>(
              start, 0, distance_between_coords(start, goal));
          open_set.push(start_node);
          node_map[start] = start_node;

          while (!open_set.empty())
          {
               // 获取f_cost最小的节点
               NodePtr current = open_set.top();
               open_set.pop();

               // 到达目标点
               if (current->coord == goal)
               {
                    return reconstruct_path(current);
               }

               // 检查相邻网格 (8个方向)
               for (int dx = -1; dx <= 1; ++dx)
               {
                    for (int dy = -1; dy <= 1; ++dy)
                    {
                         if (dx == 0 && dy == 0)
                              continue;

                         GridCoord neighbor_coord(
                             current->coord.x + dx,
                             current->coord.y + dy);

                         // 跳过障碍物
                         if (obstacles_.find(neighbor_coord) != obstacles_.end())
                         {
                              continue;
                         }

                         // 计算移动成本（对角线成本更高）
                         double move_cost = (dx != 0 && dy != 0) ? 1.414 : 1.0;
                         double tentative_g = current->g_cost + move_cost;

                         // 检查节点是否已存在
                         auto it = node_map.find(neighbor_coord);
                         bool is_new_node = (it == node_map.end());
                         bool is_better_path = false;

                         if (!is_new_node)
                         {
                              // 如果找到更优路径
                              is_better_path = tentative_g < it->second->g_cost;
                         }

                         if (is_new_node || is_better_path)
                         {
                              // 创建新节点或更新现有节点
                              double h_cost = distance_between_coords(neighbor_coord, goal);
                              auto neighbor_node = std::make_shared<AStarNode>(
                                  neighbor_coord, tentative_g, h_cost, current);

                              // 更新节点映射
                              node_map[neighbor_coord] = neighbor_node;

                              // 新节点加入开放集
                              open_set.push(neighbor_node);
                         }
                    }
               }
          }

          return {}; // 未找到路径
     }

     // 重构路径
     std::vector<GridCoord> reconstruct_path(std::shared_ptr<AStarNode> goal_node)
     {
          std::vector<GridCoord> path;
          auto current = goal_node;

          while (current != nullptr)
          {
               path.push_back(current->coord);
               current = current->parent;
          }

          std::reverse(path.begin(), path.end());
          return path;
     }

     // 发布完整路径（修复版）
     void publish_path(const std::vector<GridCoord> &grid_path)
     {
          nav_msgs::msg::Path path_msg;
          path_msg.header.stamp = now();
          path_msg.header.frame_id = "odom";

          for (size_t i = 0; i < grid_path.size(); ++i)
          {
               const auto &coord = grid_path[i];
               auto [world_x, world_y] = grid_to_world(coord.x, coord.y);

               geometry_msgs::msg::PoseStamped pose;
               pose.header = path_msg.header;
               pose.pose.position.x = world_x;
               pose.pose.position.y = world_y;
               pose.pose.position.z = 0.0;
               pose.pose.orientation.w = 1.0; // 默认朝向

               path_msg.poses.push_back(pose);
          }

          path_pub_->publish(path_msg);

          // 记录路径点信息
          RCLCPP_INFO(get_logger(), "Published path with %zu points", path_msg.poses.size());
          for (const auto &pose : path_msg.poses)
          {
               RCLCPP_INFO(get_logger(), "Point: (%.2f, %.2f)",
                           pose.pose.position.x, pose.pose.position.y);
          }
     }

     // 计算控制指令
     geometry_msgs::msg::Twist calculate_control(double target_x, double target_y)
     {
          geometry_msgs::msg::Twist cmd_vel;

          // 计算目标方向
          double target_yaw = atan2(target_y - current_position_.y,
                                    target_x - current_position_.x);
          double yaw_error = target_yaw - current_yaw_;

          // 角度归一化 (-π 到 π)
          while (yaw_error > M_PI)
               yaw_error -= 2 * M_PI;
          while (yaw_error < -M_PI)
               yaw_error += 2 * M_PI;

          // 距离误差
          double distance = std::hypot(target_x - current_position_.x,
                                       target_y - current_position_.y);

          // PID控制参数
          const double Kp_linear = 0.5;
          const double Kp_angular = 1.0;
          const double max_linear = 0.5;
          const double max_angular = 1.0;

          // 计算速度
          if (std::abs(yaw_error) > 0.1)
          { // 先调整方向
               cmd_vel.angular.z = std::clamp(Kp_angular * yaw_error, -max_angular, max_angular);
          }
          else
          { // 前进
               cmd_vel.linear.x = std::clamp(Kp_linear * distance, -max_linear, max_linear);
          }

          return cmd_vel;
     }

     // 停止机器人
     void stop_robot()
     {
          geometry_msgs::msg::Twist cmd_vel;
          cmd_vel_pub_->publish(cmd_vel);
     }

     // 世界坐标转网格坐标
     GridCoord world_to_grid(double x, double y) const
     {
          return GridCoord(
              static_cast<int>(std::round(x / grid_size_)),
              static_cast<int>(std::round(y / grid_size_)));
     }

     // 网格坐标转世界坐标
     std::pair<double, double> grid_to_world(int grid_x, int grid_y) const
     {
          return {grid_x * grid_size_, grid_y * grid_size_};
     }

     // 计算两个网格坐标之间的距离
     double distance_between_coords(const GridCoord &a, const GridCoord &b) const
     {
          int dx = a.x - b.x;
          int dy = a.y - b.y;
          return std::sqrt(dx * dx + dy * dy) * grid_size_;
     }

     // 计算两个世界点之间的距离
     double distance_between_points(const geometry_msgs::msg::Point &a,
                                    const geometry_msgs::msg::Point &b) const
     {
          return std::hypot(a.x - b.x, a.y - b.y);
     }

     // 参数
     const double grid_size_;
     const double goal_tolerance_;

     // 数据存储
     geometry_msgs::msg::Point current_position_;
     geometry_msgs::msg::Point goal_position_;
     double current_yaw_ = 0.0;
     std::unordered_set<GridCoord> obstacles_;

     // ROS2 订阅和发布
     rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
     rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr lidar_sub_;
     rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr radar_sub_;
     rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
     rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr path_pub_;
     rclcpp::TimerBase::SharedPtr planning_timer_;

     // 线程安全
     std::mutex data_mutex_;
};

int main(int argc, char **argv)
{
     rclcpp::init(argc, argv);
     auto node = std::make_shared<PathPlanner>();
     rclcpp::spin(node);
     rclcpp::shutdown();
     return 0;
}